#include "env_updater.h"

#include <iostream>
#include "dlfcn.h"
#include "ros/package.h"

#include <QDebug>
#include <QDir>

bool EnvUpdater::updateEnv() {
  auto addEnv = [](std::string name, std::string env) {
    char* tmp = getenv(name.c_str());
    std::string old_env = tmp == nullptr ? "" : tmp;

    QStringList old_env_list = QString(old_env.c_str()).split(":");
    QStringList env_list = {QString(env.c_str())};

    for (auto e : old_env_list) {
      if (!env_list.contains(e)) { env_list.append(e); }
    }
    setenv(name.c_str(), env_list.join(":").toStdString().c_str(), 1);
  };

  auto setEnv = [](const char* name, const char* env) { setenv(name, env, 1); };

  auto addROSEnv = [&](std::string package_dir) {
    addEnv("ROS_PACKAGE_PATH", package_dir + "/share");
    addEnv("LD_LIBRARY_PATH", package_dir + "/lib");
    addEnv("PATH", package_dir + "/bin");
    addEnv("PKG_CONFIG_PATH", package_dir + "/lib/pkgconfig");
    addEnv("CMAKE_PREFIX_PATH", package_dir);
    addEnv("PYTHONPATH", package_dir + "/lib/python2.7/dist-packages");
    addEnv("PYTHONPATH", package_dir + "/lib/python3/dist-packages");
  };

  addROSEnv("/opt/ros/kinetic");
  addROSEnv("/opt/ros/noetic");
  addROSEnv(QDir("./rviz").absolutePath().toStdString());

  //  if (dlopen("./rviz/lib/librviz.so", RTLD_NOW | RTLD_GLOBAL) == nullptr) {
  //    qCritical() << "failed to load librviz.so";
  //    return false;
  //  }

  //  if (dlopen("./libCanCmd.so", RTLD_NOW | RTLD_GLOBAL) == nullptr) {
  //    qCritical() << "failed to load libCanCmd.so";
  //    return false;
  //  }

  addEnv("PATH", "/opt/Qt5.14.2/5.14.2/gcc_64/bin");
  addEnv("LD_LIBRARY_PATH", "/opt/Qt5.14.2/5.14.2/gcc_64/lib");
  addEnv("LD_LIBRARY_PATH", QDir("./").absolutePath().toStdString().c_str());

  setEnv("ROS_MASTER_URI", "http://localhost:11311");
  setEnv("env_updated", "true");

  return true;
}
